#!/usr/bin/env python3
"""
USV Controller Launch File with Proper Parameter Priority
实现正确参数优先级的启动文件：命令行 > launch文件 > 配置文件
"""

import os
from datetime import datetime
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, LogInfo, ExecuteProcess
from launch_ros.actions import Node
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
from launch_ros.substitutions import FindPackageShare
from launch.conditions import IfCondition

def generate_launch_description():
    """生成启动描述"""
    
    # 声明启动参数（这些可以通过命令行覆盖）
    usv_type_arg = DeclareLaunchArgument(
        'usv_type',
        default_value='1',  # launch文件中的默认值
        description='USV type identifier: 1=single motor, 2=differential drive, 3=omnidirectional'
    )
    
    usv_id_arg = DeclareLaunchArgument(
        'usv_id',
        default_value='0',
        description='USV unique identifier'
    )
    
    control_frequency_arg = DeclareLaunchArgument(
        'control_frequency',
        default_value='10.0',
        description='Control loop frequency in Hz'
    )
    
    log_level_arg = DeclareLaunchArgument(
        'log_level',
        default_value='info',
        description='Logging level (debug, info, warn, error, fatal)'
    )
    
    config_file_arg = DeclareLaunchArgument(
        'config_file',
        default_value='usv_params.yaml',
        description='Configuration file name'
    )
    
    # 新增：是否启用配置文件（简化版本）
    use_config_file_arg = DeclareLaunchArgument(
        'use_config_file',
        default_value='true',
        description='Whether to load base parameters from config file (typically should be true)'
    )
    
    # rosbag相关参数
    enable_rosbag_arg = DeclareLaunchArgument(
        'enable_rosbag',
        default_value='true',
        description='Whether to enable rosbag recording'
    )
    
    bag_output_dir_arg = DeclareLaunchArgument(
        'bag_output_dir',
        default_value='/home/ubuntu/usv_bags',
        description='Directory to save rosbag files'
    )
    
    bag_prefix_arg = DeclareLaunchArgument(
        'bag_prefix',
        default_value='usv_mission',
        description='Prefix for rosbag file names'
    )
    
    compression_mode_arg = DeclareLaunchArgument(
        'compression_mode',
        default_value='none',
        description='Compression mode for rosbag (none, zstd, lz4)'
    )
    
    max_bag_size_arg = DeclareLaunchArgument(
        'max_bag_size',
        default_value='1000000000',  # 1GB
        description='Maximum bag file size in bytes'
    )
    
    # 配置文件路径
    config_file = PathJoinSubstitution([
        FindPackageShare('usv_controller'),
        'config',
        LaunchConfiguration('config_file')
    ])
    
    # USV控制器节点 - 推荐的简洁方案
    usv_controller_node = Node(
        package='usv_controller',
        executable='usv_controller_node',
        name='usv_controller',
        parameters=[
            config_file,  # 首先加载配置文件作为基础配置
            {
                # launch参数会覆盖配置文件中的同名参数
                # 命令行参数会进一步覆盖这些值
                'usv_type': LaunchConfiguration('usv_type'),
                'usv_id': LaunchConfiguration('usv_id'),
                'control_frequency': LaunchConfiguration('control_frequency'),
            }
        ],
        arguments=[
            '--ros-args',
            '--log-level', LaunchConfiguration('log_level')
        ],
        output='screen',
        emulate_tty=True,
        respawn=True,
        respawn_delay=2.0,
    )
    
    # 生成带时间戳的bag文件名
    timestamp = datetime.now().strftime("%Y%m%d_%H%M%S")
    
    # rosbag记录节点
    rosbag_record = ExecuteProcess(
        condition=IfCondition(LaunchConfiguration('enable_rosbag')),
        cmd=[
            'ros2', 'bag', 'record',
            '--output', [LaunchConfiguration('bag_output_dir'), '/', 
                        LaunchConfiguration('bag_prefix'), '_', timestamp],
            '--compression-mode', LaunchConfiguration('compression_mode'),
            '--max-bag-size', LaunchConfiguration('max_bag_size'),
            '/usv_state_1',
            '/usv_control_1', 
            '/usv_task_1',
            '/mavros/global_position/global',
            '/mavros/imu/data',
            '/mavros/local_position/velocity_local',
            "/mavros/rc/out"
        ],
        output='screen',
        emulate_tty=True,
        shell=False
    )
    
    # 启动信息
    launch_info = LogInfo(
        msg=[
            'Launching USV Controller with parameter priority:\n',
            '  1. Command line arguments (highest priority)\n',
            '  2. Launch file parameters\n', 
            '  3. Config file parameters (lowest priority)\n',
            '\nCurrent configuration:\n',
            '  - USV Type: ', LaunchConfiguration('usv_type'), '\n',
            '  - USV ID: ', LaunchConfiguration('usv_id'), '\n',
            '  - Control Frequency: ', LaunchConfiguration('control_frequency'), ' Hz\n',
            '  - Use Config File: ', LaunchConfiguration('use_config_file'), '\n',
            '  - Config File: ', LaunchConfiguration('config_file'), '\n',
            '  - Log Level: ', LaunchConfiguration('log_level')
        ]
    )
    
    return LaunchDescription([
        # 参数声明
        usv_type_arg,
        usv_id_arg,
        control_frequency_arg,
        log_level_arg,
        config_file_arg,
        use_config_file_arg,
        
        # rosbag参数
        enable_rosbag_arg,
        bag_output_dir_arg,
        bag_prefix_arg,
        compression_mode_arg,
        max_bag_size_arg,
        
        # 启动信息
        launch_info,
        
        # 节点
        usv_controller_node,
        rosbag_record,
    ])